#include "app.h"

static U32 calc_bit_width(U32 bit_end);

//
// Examples:
//      char* user_cmd = "r 0x60004000 0x70 25:0";
//      char* user_cmd = "w 0x60004070 0x34 8:0 0x5";
//
void frame_handler(S8* user_cmd, U32 user_cmd_len, U32* result)
{
    RETCODE           retcode;
    struct user_cmd_t cmd;
    struct mem_op*    op;
    U32               reg_value = 0;

    op = mem_op_get(MEM_OP_SWD);
    GOTO_FAILED(NULL == op);

    retcode = user_cmd_parse(user_cmd, &cmd);
    GOTO_FAILED(-1 == retcode);

    cmd.bit_width = calc_bit_width(cmd.bit_end);

    retcode = op->reg_rd(cmd.reg_addr, cmd.bit_width, &reg_value);
    GOTO_FAILED(0 != retcode);

    // 2: Write back
    if ('w' == cmd.cmd)
    {
        BITSET(reg_value, cmd.bit_start, cmd.bit_end, cmd.bit_value);

        retcode = op->reg_wr(cmd.reg_addr, cmd.bit_width, reg_value);
        GOTO_FAILED(0 != retcode);

        retcode = op->reg_rd(cmd.reg_addr, cmd.bit_width, &reg_value);
        GOTO_FAILED(0 != retcode);
    }

    *result = BITGET(reg_value, cmd.bit_start, cmd.bit_end);

    return;

FAILED:
    *result = 0xFFFFFFFF;
}

static U32 calc_bit_width(U32 bit_end)
{
    U32 bit_width;

    if (bit_end < 8)
    {
        bit_width = 8;
    }
    else if (bit_end < 16)
    {
        bit_width = 16;
    }
    else
    {
        bit_width = 32;
    }

    return bit_width;
}